#include <iostream>
#include "Bloomenthal.h"

//Define static members of bloomenthal
Bloomenthal::INTLISTS* Bloomenthal::cubetable[256];
int Bloomenthal::corner1[12]	 = {BLM_LBN,BLM_LTN,BLM_LBN,BLM_LBF,BLM_RBN,BLM_RTN,BLM_RBN,BLM_RBF,BLM_LBN,BLM_LBF,BLM_LTN,BLM_LTF};
int Bloomenthal::corner2[12]	 = {BLM_LBF,BLM_LTF,BLM_LTN,BLM_LTF,BLM_RBF,BLM_RTF,BLM_RTN,BLM_RTF,BLM_RBN,BLM_RBF,BLM_RTN,BLM_RTF};
int Bloomenthal::leftface[12]	 = {BLM_B,  BLM_L,  BLM_L,  BLM_F,  BLM_R,  BLM_T,  BLM_N,  BLM_R,  BLM_N,  BLM_B,  BLM_T,  BLM_F};
int Bloomenthal::rightface[12] = {BLM_L,  BLM_T,  BLM_N,  BLM_L,  BLM_B,  BLM_R,  BLM_R,  BLM_F,  BLM_B,  BLM_F,  BLM_N,  BLM_T};

Bloomenthal::Bloomenthal()
{

}

Bloomenthal::~Bloomenthal()
{

}

/**** An Implicit Surface Polygonizer ****/


/* polygonize: polygonize the implicit surface function
 *   arguments are:
 *	 double function (x, y, z)
 *		 double x, y, z (an arbitrary 3D point)
 *	     the implicit surface function
 *	     return negative for inside, positive for outside
 *	 double x, y, z
 *	     coordinates of a starting point on or near the surface
 *	     may be defaulted to 0., 0., 0.
 *   returns error or NULL
 */


int Bloomenthal::polygonize(VERTICES &vertexlist, CUBELIST* &cubelist, double &x, double &y, double &z, const double gsize)
{
  int cubecount = 0;
  srand(1);
  vertexlist.count = vertexlist.max = 0;
  vertexlist.ptr = NULL;
	cubelist = NULL;

	PROCESS p;
  int n;

  p.size = gsize;//mesh->GetGridSize();

  /* allocate hash tables and build cube polygon table: */
  p.centers = (CENTERLIST **) mycalloc(BLM_HASHSIZE,sizeof(CENTERLIST *));
  p.corners = (CORNERLIST **) mycalloc(BLM_HASHSIZE,sizeof(CORNERLIST *));
  p.edges =	(EDGELIST   **) mycalloc(2*BLM_HASHSIZE,sizeof(EDGELIST *));

  makecubetable();

  // Round x,y of origin to get consistant grid space
  p.start.x = floor(x/gsize) * gsize;
  p.start.y = floor(y/gsize) * gsize;

  // find point above and below surface (z values) @ position (x,y)
  double zabove = z, valueabove = 0;
  bool foundabove = find1(1, p.size, p.start.x, p.start.y, zabove, valueabove);
  double zbelow = zabove, valuebelow = 0;
  bool foundbelow = find1(0, p.size, p.start.x, p.start.y, zbelow, valuebelow);

  if (!foundabove || !foundbelow) 
    return cubecount;
  else
    converge(p.start.x, p.start.y, zabove, zbelow, BLM_RES);

  // Round z of origin to get consistant grid space but keep crossing inside cube
  p.start.z = floor(p.start.z/p.size) * p.size;

  /* push initial cube on stack: */
  p.cubes = (CUBES *) mycalloc(1, sizeof(CUBES)); /* list of 1 */
  p.cubes->cube.i = p.cubes->cube.j = p.cubes->cube.k = 0;
  p.cubes->next = NULL;

  /* set corners of initial cube: */
  for (n = 0; n < 8; n++)
	  p.cubes->cube.corners[n] = setcorner1(&p, BLM_BIT(n,2), BLM_BIT(n,1), BLM_BIT(n,0));

  p.vertices.count = p.vertices.max = 0; /* no vertices yet */
  p.vertices.ptr = NULL;

  setcenter(p.centers, 0, 0, 0);

  while (p.cubes != NULL)  // process active cubes till none left
  {
	  CUBE c = p.cubes->cube;

	  /// test six face directions (before triangulating so know if is boundary cube)
	  FaceTest face1 = testface(c.i-1, c.j, c.k, &c, BLM_LBN, BLM_LBF, BLM_LTN, BLM_LTF, &p);
	  FaceTest face2 = testface(c.i+1, c.j, c.k, &c, BLM_RBN, BLM_RBF, BLM_RTN, BLM_RTF, &p);
	  FaceTest face3 = testface(c.i, c.j-1, c.k, &c, BLM_LBN, BLM_LBF, BLM_RBN, BLM_RBF, &p);
	  FaceTest face4 = testface(c.i, c.j+1, c.k, &c, BLM_LTN, BLM_LTF, BLM_RTN, BLM_RTF, &p);
	  FaceTest face5 = testface(c.i, c.j, c.k-1, &c, BLM_LBN, BLM_LTN, BLM_RBN, BLM_RTN, &p);
	  FaceTest face6 = testface(c.i, c.j, c.k+1, &c, BLM_LBF, BLM_LTF, BLM_RBF, BLM_RTF, &p);

//std::cout << face1 << " " << face2 << " " << face3 << " " << face4 << " " << face5 << " " << face6 << std::endl;

    //If current cube is good and any neighbouring cube is out of bounds or bad, then current cube is on boundary of mesh
    if(c.good && (face1 == OUT_OF_BOUNDS || face2 == OUT_OF_BOUNDS || face3 == OUT_OF_BOUNDS || face4 == OUT_OF_BOUNDS || face5 == OUT_OF_BOUNDS || face6 == OUT_OF_BOUNDS || face1 == BAD_CUBE || face2 == BAD_CUBE || face3 == BAD_CUBE || face4 == BAD_CUBE || face5 == BAD_CUBE || face6 == BAD_CUBE || face1 == BAD_VISITED || face2 == BAD_VISITED || face3 == BAD_VISITED || face4 == BAD_VISITED || face5 == BAD_VISITED || face6 == BAD_VISITED))
    {
      CUBELIST* new_cube = new CUBELIST;
      new_cube->next = cubelist;
      cubelist = new_cube;
      cubecount++;

      new_cube->triscount = 0;
      new_cube->ci = c.i;
      new_cube->cj = c.j;
      new_cube->ck = c.k;

  	  docube(vertexlist, new_cube->tris, new_cube->triscount, &c, &p, true);
    }
    else if(c.good) //ignore bad cubes, but consider their neighbours
    {
      CUBELIST* new_cube = new CUBELIST;
      new_cube->next = cubelist;
      cubelist = new_cube;
      cubecount++;

      new_cube->triscount = 0;
      new_cube->ci = c.i;
      new_cube->cj = c.j;
      new_cube->ck = c.k;

      docube(vertexlist, new_cube->tris, new_cube->triscount, &c, &p, false);
    }

	  // pop current cube from stack
	  CUBES *temp = p.cubes;
	  p.cubes = p.cubes->next;
	  free((char *) temp);
    
    //if neighbouring cubes are valid add them to the stack
    if(face1 == GOOD_CUBE)
      addcube(c.i-1, c.j, c.k, &p, true);
    else if(face1 == BAD_CUBE)
      addcube(c.i-1, c.j, c.k, &p, false);
    
    if(face2 == GOOD_CUBE)
      addcube(c.i+1, c.j, c.k, &p, true);
    else if(face1 == BAD_CUBE)
      addcube(c.i+1, c.j, c.k, &p, false);

    if(face3 == GOOD_CUBE)
      addcube(c.i, c.j-1, c.k, &p, true);
    else if(face1 == BAD_CUBE)
      addcube(c.i, c.j-1, c.k, &p, false);

    if(face4 == GOOD_CUBE)
      addcube(c.i, c.j+1, c.k, &p, true);
    else if(face1 == BAD_CUBE)
      addcube(c.i, c.j+1, c.k, &p, false);

    if(face5 == GOOD_CUBE)
      addcube(c.i, c.j, c.k-1, &p, true);
    else if(face1 == BAD_CUBE)
      addcube(c.i, c.j, c.k-1, &p, false);

    if(face6 == GOOD_CUBE)
      addcube(c.i, c.j, c.k+1, &p, true);
    else if(face1 == BAD_CUBE)
      addcube(c.i, c.j, c.k+1, &p, false);

	  for(n=0; n<8; n++)
		  free((char *)c.corners[n]);
  }

	//free
	for(unsigned int i=0; i<BLM_HASHSIZE; i++){
		while(p.centers[i] != NULL){
			CENTERLIST* current = p.centers[i];
			p.centers[i] = current->next;
			free((char*)current);
		}
	}
	free((char*)p.centers);

	for(unsigned int i=0; i<BLM_HASHSIZE; i++){
		while(p.corners[i] != NULL){
			CORNERLIST* current = p.corners[i];
			p.corners[i] = current->next;
			free((char*)current);
		}
	}
	free((char*)p.corners);

	for(unsigned int i=0; i<2*BLM_HASHSIZE; i++){
		while(p.edges[i] != NULL){
			EDGELIST* current = p.edges[i];
			p.edges[i] = current->next;
			free((char*)current);
		}
	}
  free((char*)p.edges);

  return cubecount;
}

void Bloomenthal::triproc(TRILIST* &trilist, int i1, int i2, int i3, bool b1, bool b2, bool b3)//int ci, int cj, int ck, int cf, 
{

	TRILIST* new_tris = new TRILIST;
  new_tris->next = trilist;
  trilist = new_tris;

  /*if(trilist == NULL)
    trilist = tris_tail = new_tris;
  else
    tris_tail->next = new_tris;*/

	//new_tris->next = NULL;
	//new_tris->ci = ci;
	//new_tris->cj = cj;
	//new_tris->ck = ck;
  //new_tris->cf = cf;
	new_tris->i1 = i1;
	new_tris->i2 = i2;
	new_tris->i3 = i3;
	new_tris->b1 = b1;
	new_tris->b2 = b2;
	new_tris->b3 = b3;
	//tris_tail = new_tris;


}	

/* testface: given cube at lattice (i, j, k), and four corners of face,
 * if surface crosses face, compute other four corners of adjacent cube
 * and add new cube to cube stack 
 * returns FaceType */

Bloomenthal::FaceTest Bloomenthal::testface(int i, int j, int k, CUBE *old, int c1, int c2, int c3, int c4, PROCESS *p)
{
  int pos = old->corners[c1]->value > 0.0 ? 1 : 0;

  if ((old->corners[c2]->value > 0) == pos && (old->corners[c3]->value > 0) == pos && (old->corners[c4]->value > 0) == pos) // test if no surface crossing
    return NO_CROSSING;
	else
  {
    BoundaryTest bt = func->testBounds(i*p->size + p->start.x, j*p->size + p->start.y, k*p->size + p->start.z);
    
    if (bt == OUTSIDE_BB)
    {
      old->corners[c1]->shared = true;
      old->corners[c2]->shared = true;
      old->corners[c3]->shared = true;
      old->corners[c4]->shared = true;
      return OUT_OF_BOUNDS;
    }    
    else if (setcenter(p->centers, i, j, k))    // test if already visited
    {
      if (bt == OUTSIDE_TIGHT)     
      {
        old->corners[c1]->shared = true;
        old->corners[c2]->shared = true;
        old->corners[c3]->shared = true;
        old->corners[c4]->shared = true; 
        return BAD_VISITED;
      }      
      else //if (bt ==INSIDE)
        return GOOD_VISITED;
    }
    else if (bt == OUTSIDE_TIGHT)
    {
      old->corners[c1]->shared = true;
      old->corners[c2]->shared = true;
      old->corners[c3]->shared = true;
      old->corners[c4]->shared = true;
      return BAD_CUBE;
    }
    else //if (bt ==INSIDE)
      return GOOD_CUBE;
  }
}

void Bloomenthal::addcube(int i, int j, int k, PROCESS *p, bool good)
{
  /* create new cube: */
	CUBE new1;
  new1.i = i;
  new1.j = j;
  new1.k = k;
  new1.good = good;

  for (int n = 0; n < 8; n++)
    new1.corners[n] = setcorner1(p, i+BLM_BIT(n,2), j+BLM_BIT(n,1), k+BLM_BIT(n,0));

  /*add cube to top of stack: */
  CUBES *oldcubes = p->cubes;
  p->cubes = (CUBES *) mycalloc(1, sizeof(CUBES));
  p->cubes->cube = new1;
  p->cubes->next = oldcubes;
}

/* setcorner: return corner with the given lattice location
   set (and cache) its function value */
Bloomenthal::CORNER * Bloomenthal::setcorner1(PROCESS *p, int i, int j, int k)
{
	/* for speed, do corner value caching here */
    CORNER *c = (CORNER *) mycalloc(1, sizeof(CORNER));
    int index = BLM_HASH(i, j, k);
    CORNERLIST *l = p->corners[index];
    c->i = i; c->x = p->start.x+((double)i-.5)*p->size;
    c->j = j; c->y = p->start.y+((double)j-.5)*p->size;
    c->k = k; c->z = p->start.z+((double)k-.5)*p->size;
    c->shared = false;
    for (; l != NULL; l = l->next)
	if (l->i == i && l->j == j && l->k == k) {
	    c->value = l->value;
	    return c;
	    }
    l = (CORNERLIST *) mycalloc(1, sizeof(CORNERLIST));
    l->i = i; l->j = j; l->k = k;
    l->value = c->value = func->value(c->x, c->y, c->z);
    l->next = p->corners[index];
    p->corners[index] = l;
    return c;
}

/* find: search for point with value of given sign (0: neg, 1: pos) */
bool Bloomenthal::find1(const int sign, const double gsize, const double x, const double y, double &z, double &value)
{
  bool found = false;
  double rscale = gsize;

  for (int i = 0; i < 10000; i++) 
  {
    double tz = z + rscale*(BLM_RAND()-0.5);
    value = func->value(x, y, tz);

    if (sign == (value > 0.0)) 
    {
      z = tz;
      found = true;
      break;
    }
    else
      rscale = rscale*1.0005; // slowly expand search outwards
  }

  return found;

	/*int i;
    TEST test;
    test.ok = 1;
    for (i = 0; i < 10000; i++) {
	test.p.x = x+gsize*(BLM_RAND()-0.5);
	test.p.y = y+gsize*(BLM_RAND()-0.5);
	test.p.z = z+gsize*(BLM_RAND()-0.5);
	test.value = func->value(test.p.x, test.p.y, test.p.z);
	if (sign == (test.value > 0.0)) return test;
	gsize = gsize*1.0005; // slowly expand search outwards
    }
    test.ok = 0;
    return test;*/
}


/* docube: triangulate the cube directly, without decomposition */
void Bloomenthal::docube(VERTICES &vertexlist, TRILIST* &trilist, int &tricount, CUBE *cube, PROCESS *p, bool boundary)
{
	INTLISTS *polys;
  int i, index = 0;
  for (i = 0; i < 8; i++) 
    if (cube->corners[i]->value > 0.0) 
      index += (1<<i);

  //int face = 0;
  for (polys = cubetable[index]; polys; polys = polys->next) {
  	INTLIST *edges;
  	int a = -1, b = -1, c = -1, count = 0;
    bool ba = false, bb = false, bc = false;
  	for (edges = polys->list; edges; edges = edges->next) {
	    CORNER *c1 = cube->corners[corner1[edges->i]];
	    CORNER *c2 = cube->corners[corner2[edges->i]];
	    c = vertid(c1, c2, p);
      bc = c1->shared && c2->shared;
	    if (++count > 2) 
      {
      	vertexlist = p->vertices;
        tricount++;
        if(boundary)
          triproc(trilist, a, b, c, ba, bb, bc);//cube->i, cube->j, cube->k, face++, 
        else
          triproc(trilist, a, b, c, false, false, false);//cube->i, cube->j, cube->k, face++, 
      }	    
      else
      {
        a = b;
        ba = bb;
      }
	    b = c;
      bb = bc;
  	}
  }
}

/* nextcwedge: return next clockwise edge from given edge around given face */

int Bloomenthal::nextcwedge(int edge, int face)
{
	switch (edge) {
	case BLM_LB: return (face == BLM_L)? BLM_LF : BLM_BN;
	case BLM_LT: return (face == BLM_L)? BLM_LN : BLM_TF;
	case BLM_LN: return (face == BLM_L)? BLM_LB : BLM_TN;
	case BLM_LF: return (face == BLM_L)? BLM_LT : BLM_BF;
	case BLM_RB: return (face == BLM_R)? BLM_RN : BLM_BF;
	case BLM_RT: return (face == BLM_R)? BLM_RF : BLM_TN;
	case BLM_RN: return (face == BLM_R)? BLM_RT : BLM_BN;
	case BLM_RF: return (face == BLM_R)? BLM_RB : BLM_TF;
	case BLM_BN: return (face == BLM_B)? BLM_RB : BLM_LN;
	case BLM_BF: return (face == BLM_B)? BLM_LB : BLM_RF;
	case BLM_TN: return (face == BLM_T)? BLM_LT : BLM_RN;
	case BLM_TF: return (face == BLM_T)? BLM_RT : BLM_LF;
	default: return 0;
    }
}

/* otherface: return face adjoining edge that is not the given face */

int Bloomenthal::otherface(int edge, int face)
{
	int other = leftface[edge];
    return face == other? rightface[edge] : other;
}

/* makecubetable: create the 256 entry table for cubical polygonization */
void Bloomenthal::makecubetable()
{
	static int visited = 0;

	if (visited)
		return;

	visited = 1;

	int i, e, c, done[12], pos[8];
    for (i = 0; i < 256; i++) {
	for (e = 0; e < 12; e++) done[e] = 0;
	for (c = 0; c < 8; c++) pos[c] = BLM_BIT(i, c);
	for (e = 0; e < 12; e++)
	    if (!done[e] && (pos[corner1[e]] != pos[corner2[e]])) {
		INTLIST *ints = 0;
		INTLISTS *lists = (INTLISTS *) mycalloc(1, sizeof(INTLISTS));
		int start = e, edge = e;
		/* get face that is to right of edge from pos to neg corner: */
		int face = pos[corner1[e]]? rightface[e] : leftface[e];
		while (1) {
		    edge = nextcwedge(edge, face);
		    done[edge] = 1;
		    if (pos[corner1[edge]] != pos[corner2[edge]]) {
			INTLIST *tmp = ints;
			ints = (INTLIST *) mycalloc(1, sizeof(INTLIST));
			ints->i = edge;
			ints->next = tmp; /* add edge to head of list */
			if (edge == start) break;
			face = otherface(edge, face);
		    }
		}
		lists->list = ints; /* add ints to head of table entry */
		lists->next = cubetable[i];
		cubetable[i] = lists;
	    }
    }
}

/* mycalloc: return successful calloc or exit program */
char * Bloomenthal::mycalloc(int nitems, int nbytes)
{
	char *ptr = (char*)calloc(nitems, nbytes);
	if (ptr != NULL) return ptr;
	fprintf(stderr, "can't calloc %d bytes\n", nitems*nbytes);
	exit(1);
}


/* setcenter: set (i,j,k) entry of table[]
 * return 1 if already set; otherwise, set and return 0 */
int Bloomenthal::setcenter(CENTERLIST *table[], int i, int j, int k)
{
	int index = BLM_HASH(i, j, k);
  CENTERLIST *new1, *l, *q = table[index];
  for (l = q; l != NULL; l = l->next)
	  if (l->i == i && l->j == j && l->k == k) 
      return 1;

  new1 = (CENTERLIST *) mycalloc(1, sizeof(CENTERLIST));
  new1->i = i; new1->j = j; new1->k = k; new1->next = q;
  table[index] = new1;
  return 0;
}

/* setedge: set vertex id for edge */
void Bloomenthal::setedge(EDGELIST *table[], int i1, int j1, int k1, int i2, int j2, int k2, int vid)
{
	unsigned int index;
  EDGELIST *new1;
  if (i1>i2 || (i1==i2 && (j1>j2 || (j1==j2 && k1>k2)))) {
  	int t=i1; i1=i2; i2=t; t=j1; j1=j2; j2=t; t=k1; k1=k2; k2=t;
  }
  index = BLM_HASH(i1, j1, k1) + BLM_HASH(i2, j2, k2);
  new1 = (EDGELIST *) mycalloc(1, sizeof(EDGELIST));
  new1->i1 = i1; new1->j1 = j1; new1->k1 = k1;
  new1->i2 = i2; new1->j2 = j2; new1->k2 = k2;
  new1->vid = vid;
  new1->next = table[index];
  table[index] = new1;	
}

/* getedge: return vertex id for edge; return -1 if not set */
int Bloomenthal::getedge(EDGELIST *table[], int i1, int j1, int k1, int i2, int j2, int k2)
{
	EDGELIST *q;
  if (i1>i2 || (i1==i2 && (j1>j2 || (j1==j2 && k1>k2)))) {
	  int t=i1; i1=i2; i2=t; t=j1; j1=j2; j2=t; t=k1; k1=k2; k2=t;
  };
  q = table[BLM_HASH(i1, j1, k1)+BLM_HASH(i2, j2, k2)];
  for (; q != NULL; q = q->next)
	  if (q->i1 == i1 && q->j1 == j1 && q->k1 == k1 && q->i2 == i2 && q->j2 == j2 && q->k2 == k2)
	    return q->vid;

  return -1;
}


/* vertid: return index for vertex on edge:
 * c1->value and c2->value are presumed of different sign
 * return saved index if any; else compute vertex and save */
int Bloomenthal::vertid(CORNER *c1, CORNER *c2, PROCESS *p)
{
	VERTEX v;
  POINT1 a, b;
  int vid = getedge(p->edges, c1->i, c1->j, c1->k, c2->i, c2->j, c2->k);
  if (vid != -1) 
    return vid;			     /* previously computed */
  a.x = c1->x; a.y = c1->y; a.z = c1->z;
  b.x = c2->x; b.y = c2->y; b.z = c2->z;

	double x1, y1, z1, x2, y2, z2, f1, f2;
	if(c1->value > 0){
		f1 = c1->value;
		x1 = c1->x; y1 = c1->y; z1 = c1->z;
		f2 = c2->value;
		x2 = c2->x; y2 = c2->y; z2 = c2->z;
	}
	else{
		f1 = c2->value;
		x1 = c2->x; y1 = c2->y; z1 = c2->z;
		f2 = c1->value;
		x2 = c1->x; y2 = c1->y; z2 = c1->z;
	}

	//Repeated Linear Interpolation
	double x, y, z, f;
	for(unsigned int i=0; i<10; i++){
		double w1 = fabs(f1);
		double w2 = fabs(f2);
		x = (w2*x1 + w1*x2)/(w1+w2);
		y = (w2*y1 + w1*y2)/(w1+w2);
		z = (w2*z1 + w1*z2)/(w1+w2);
		break;

		f = func->value(x,y,z);
		if(fabs(f) < 0.001)
			break;
		if(f > 0){
			f1 = f;
			x1 = x;  y1 = y;  z1 = z;
		}
		else{
			f2 = f;
			x2 = x;  y2 = y;  z2 = z;
		}
	}
	v.position.x = x;
	v.position.y = y;
	v.position.z = z;

  addtovertices(&p->vertices, v);			   /* save vertex */
  vid = p->vertices.count-1;
  setedge(p->edges, c1->i, c1->j, c1->k, c2->i, c2->j, c2->k, vid);
  return vid;
}


/* addtovertices: add v to sequence of vertices */
void Bloomenthal::addtovertices(VERTICES *vertices, VERTEX v)
{
	if(vertices->count == vertices->max) {
		int i;

		vertices->max = vertices->count == 0 ? 10 : 2*vertices->count;
		VERTEX * new1 = (VERTEX *) mycalloc((unsigned) vertices->max, sizeof(VERTEX));
		for (i = 0; i < vertices->count; i++) 
      new1[i] = vertices->ptr[i];

		if (vertices->ptr != NULL) free((char *)vertices->ptr);
  		vertices->ptr = new1;
	}
	vertices->ptr[vertices->count++] = v;
}


// converge: from two points of differing sign, converge to zero crossing
void Bloomenthal::converge(const double x, const double y, double &zabove, double &zbelow, int iterations)
{
	int i = 0;
  while(i++ < iterations) 
  {
  	double z = 0.5*(zabove + zbelow);

  	if(func->value(x, y, z) > 0.0)
      zabove = z;
	  else
      zbelow = z;
  }
}

/*void Bloomenthal::computeFaceNormal()
{
	for(unsigned int i=0; i<face_N; i++)
  {
		if(face[i][0] < 0)
			continue;
		double nor[3];
		nor[0] = nor[1] = nor[2] = 0;
		for(unsigned int j=0; j<3; j++){
			int i1 = face[i][j];
			int i2 = face[i][(j+1)%3];
			nor[0] += (vertex[i1][1] - vertex[i2][1])*(vertex[i1][2] + vertex[i2][2]);
			nor[1] += (vertex[i1][2] - vertex[i2][2])*(vertex[i1][0] + vertex[i2][0]);
			nor[2] += (vertex[i1][0] - vertex[i2][0])*(vertex[i1][1] + vertex[i2][1]);
		}
		double len = sqrt(nor[0]*nor[0] + nor[1]*nor[1] + nor[2]*nor[2]);
		if((float)len != 0){
			normal_f[i][0] = (float)(nor[0]/len);
			normal_f[i][1] = (float)(nor[1]/len);
			normal_f[i][2] = (float)(nor[2]/len);
		}
		else{
			normal_f[i][0] = normal_f[i][1] = normal_f[i][2] = 0;
		}
	}
}*/
